#include "CollisionEntity.h"

CollisionEntityCircle::CollisionEntityCircle(real radius, Entity *parent) : Circle(radius)
{
    updatePosition(Vector2());
    setParent(parent);
}

bool CollisionEntityCircle::contain(CollisionEntity *entity) const
{
    CollisionEntityCircle *circle = dynamic_cast<CollisionEntityCircle *>(entity);
    if (circle != nullptr)
    {
        if (circle->radius > radius)
        {
            return false;
        }
        real r2 = (radius - circle->radius);
        r2 *= r2;
        return r2 - (circle->position - position).length2() <= 0;
    }
    return false;
}

Vector2 CollisionEntityCircle::contact(CollisionEntity *entity) const
{
    if (entity == nullptr)
    {
        return Vector2();
    }
    CollisionEntityCircle *circle = dynamic_cast<CollisionEntityCircle *>(entity);
    if (circle != nullptr)
    {
        real r = (circle->radius + radius);
        real r2 = r * r;
        Vector2 v = (circle->position - position);
        real inner2 = r2 - v.length2();
        if (inner2 > 0)
        {
            if (v == Vector2())
            {
                v = Vector2(so_rand(-2, 2), so_rand(-2, 2));
            }
            return v.normalized() * (r - v.length());
        }
        return Vector2();
    }
    return Vector2();
}

void CollisionEntityCircle::updatePosition(const Vector2 &pos)
{
    position = pos;
    aabb.ul = Vector2(pos.x - radius, pos.y - radius);
    aabb.dr = Vector2(pos.x + radius, pos.y + radius);
}

CollisionEntityRectangle::CollisionEntityRectangle(const Vector2 &size, Entity *parent) : Rectangle(size)
{
    updatePosition(Vector2());
    setParent(parent);
}

bool CollisionEntityRectangle::contain(CollisionEntity *entity) const
{
    const AABB *aabb2 = entity->getAABB();
    return !(aabb.ul.x > aabb2->ul.x || aabb.ul.y > aabb2->ul.y || aabb.dr.x < aabb2->dr.x || aabb.dr.y < aabb2->dr.y);
}

Vector2 CollisionEntityRectangle::contact(CollisionEntity *entity) const
{
    return Vector2();
}

void CollisionEntityRectangle::updatePosition(const Vector2 &pos)
{
    position = pos;
    aabb.ul = pos;
    aabb.dr = size + pos;
}

Vector2 CollisionEntity::getPosition() const
{
    return position;
}

Entity *CollisionEntity::getParent()
{
    return parent;
}

void CollisionEntity::setParent(Entity *parent)
{
    this->parent = parent;
}

const AABB *CollisionEntity::getAABB() const
{
    return &aabb;
}

void CollisionEntity::setFixed(bool val)
{
    fixed = val;
}

bool CollisionEntity::isFixed()
{
    return fixed;
}
